
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.buttons.DigitalIOButton;
import edu.wpi.first.wpilibj.templates.commands.DoMoveLifter;
import edu.wpi.first.wpilibj.templates.commands.DoMoveShooter;
//import edu.wpi.first.wpilibj.templates.commands.DoDriveDistance;
import edu.wpi.first.wpilibj.templates.commands.DoMoveFeeder;
import edu.wpi.first.wpilibj.templates.commands.DoWedgeMove;
import edu.wpi.first.wpilibj.templates.commands.DoPitchShooter;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.templates.commands.*;


/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OperatorInterface {
    
    private static final double LIFT_SPEED = 0.7;
    
    private static final double SHOOTER_SPEED = 0.2;
       
    private static final double FEEDER_SPEED = 1.0;


    
        
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
    // Joystick stick = new Joystick(port);
    // Button button = new JoystickButton(stick, buttonNumber);
    
    // Another type of button you can create is a DigitalIOButton, which is
    // a button or switch hooked up to the cypress module. These are useful if
    // you want to build a customized operator interface.
    // Button button = new DigitalIOButton(1);
    
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenPressed(new ExampleCommand());
    
    // Run the command while the button is being held down and interrupt it once
    // the button is released.
    // button.whileHeld(new ExampleCommand());
    
    // Start the command when the button is released  and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenReleased(new ExampleCommand());
     
    Joystick DriverJoystickLeft = new Joystick(HardwareMap.DRIVER_JOYSTICK_LEFT);
    Joystick DriverJoystickRight = new Joystick(HardwareMap.DRIVER_JOYSTICK_RIGHT);
    Joystick ShooterJoystick = new Joystick(HardwareMap.SHOOTER_JOYSTICK);


    Button buttonL1 = new JoystickButton(DriverJoystickLeft,1);
    Button buttonL2 = new JoystickButton(DriverJoystickLeft,2);
    Button buttonL3 = new JoystickButton(DriverJoystickLeft,3);
    Button buttonL4 = new JoystickButton(DriverJoystickLeft,4);
    Button buttonL5 = new JoystickButton(DriverJoystickLeft,5);
    Button buttonL6 = new JoystickButton(DriverJoystickLeft,6);
    Button buttonL7 = new JoystickButton(DriverJoystickLeft,7);
    Button buttonL8 = new JoystickButton(DriverJoystickLeft,8);
    Button buttonL9 = new JoystickButton(DriverJoystickLeft,9);
    Button buttonL10 = new JoystickButton(DriverJoystickLeft,10);
    Button buttonL11 = new JoystickButton(DriverJoystickLeft, 11);

    Button buttonR1 = new JoystickButton(DriverJoystickRight,1);
    Button buttonR2 = new JoystickButton(DriverJoystickRight,2);
    Button buttonR3 = new JoystickButton(DriverJoystickRight,3);
    Button buttonR4 = new JoystickButton(DriverJoystickRight,4);
    Button buttonR5 = new JoystickButton(DriverJoystickRight,5);
    Button buttonR6 = new JoystickButton(DriverJoystickRight,6);
    Button buttonR7 = new JoystickButton(DriverJoystickRight,7);
    Button buttonR8 = new JoystickButton(DriverJoystickRight,8);
    Button buttonR9 = new JoystickButton(DriverJoystickRight,9);
    Button buttonR10 = new JoystickButton(DriverJoystickRight,10);
    Button buttonR11 = new JoystickButton(DriverJoystickRight, 11);

    Button buttonS1 = new JoystickButton(ShooterJoystick,1);
    Button buttonS2 = new JoystickButton(ShooterJoystick,2);
    Button buttonS3 = new JoystickButton(ShooterJoystick,3);
    Button buttonS4 = new JoystickButton(ShooterJoystick,4);
    Button buttonS5 = new JoystickButton(ShooterJoystick,5);
    Button buttonS6 = new JoystickButton(ShooterJoystick,6);
    Button buttonS7 = new JoystickButton(ShooterJoystick,7);
    Button buttonS8 = new JoystickButton(ShooterJoystick,8);
    Button buttonS9 = new JoystickButton(ShooterJoystick,9);
    Button buttonS10 = new JoystickButton(ShooterJoystick,10);
    Button buttonS11 = new JoystickButton(ShooterJoystick, 11);
    
    public OperatorInterface(){
        
        //The following are the controls for the Left Driver Joystick
        //When this button is held it acctivates the lifter picking up balls 
        //from the ground
        //buttonL1.whenPressed(new DoMoveLifter(LIFT_SPEED));
        //buttonL1.whenReleased(new DoMoveLifter(0));
        //When this button is held it moves the lifter in a reverse direction
        //buttonL2.whenPressed(new DoMoveLifter(-LIFT_SPEED));
        //buttonL2.whenReleased(new DoMoveLifter(0));
        //When pressed this button it deploys the wedge
        //buttonL6.whenPressed(new DoWedgeMove(1.0));
        //buttonL6.whenReleased(new DoWedgeMove(0.0));
       
        //The following are the controls for the Right Driver Joystick 
        //When this button is held it moves the feeder foward feeding balls
        //to the shooter  
        //buttonR1.whenPressed(new DoMoveFeeder(-FEEDER_SPEED));  
        //buttonR1.whenReleased(new DoMoveFeeder(0.0));
        //When this button is held it moves the feeder in reverse
        //buttonR2.whenPressed(new DoMoveFeeder(FEEDER_SPEED));
        //buttonR2.whenReleased(new DoMoveFeeder(0.0));

       //The following are the controls for the Shooter Joystick
       //When this button is held it acctivates the shooter at the speed set 
       //by the throttle axis on the shooter joystick
 
       //*** NOTE: Pitch still isn't right...  NEED WORK HERE....
       buttonS1.whenPressed(new DoMoveFeeder(-FEEDER_SPEED));  
       buttonS1.whenReleased(new DoMoveFeeder(0.0));
       buttonS2.whenPressed(new DoPitchShooter());
     
       //When this button is pressed it deploys the wedge
       buttonS3.whenPressed(new DoMoveLifter(LIFT_SPEED));
       buttonS3.whenReleased(new DoMoveLifter(0.0));
       buttonS4.whenPressed(new DoMoveLifter(-LIFT_SPEED));  
       buttonS4.whenReleased(new DoMoveLifter(0.0));
       //When this button is held it moves the feeder in reverse
       buttonS5.whenPressed(new DoMoveFeeder(FEEDER_SPEED));
       buttonS5.whenReleased(new DoMoveFeeder(0.0));
       buttonS6.whenPressed(new DoWedgeMove(1.0));
       buttonS6.whenReleased(new DoWedgeMove(0.0));
       
        }
    public double GetDriverJoystickMove(){
        return DriverJoystickLeft.getX();
    }
    public double GetDriverJoystickRotate(){
        return DriverJoystickLeft.getY();
    }
    public double GetDriverJoystickLeftMove(){
        return DriverJoystickLeft.getY();
    }
    public double GetDriverJoystickRightMove(){
        return DriverJoystickRight.getY();
    }
    public double GetShooterJoysitckZ(){
        return (-ShooterJoystick.getZ()+1.0)/2;
    }
    public double GetShooterJoystickY(){
        return(ShooterJoystick.getY());
    }
 }

